# -*- coding: utf-8 -*-
'''
Created on 21.06.2019

@author: yu03
'''


from scipy import signal
from FFT_Interpolation import *
from Interpolation_Test import x, tau0, N

f = 20.2
phi = 0
sig = np.cos(2*np.pi*f*x + phi)
window_blackman = signal.blackman(N)
freqline, sig_FFT, sig_freq, sig_phase = FFT_cal(sig, tau0)
sig_blackman = sig * window_blackman

freqline_blackman, sig_FFT_blackman, sig_freq_blackman, sig_phase_blackman = FFT_cal(sig_blackman, tau0)

freq_bin = 1/ tau0 / N
m_k_num = sig_freq_blackman.argmax() ### frequency bin 序数
m_k = freqline_blackman[m_k_num] ### 换算为实际频率
X_m_k = sig_FFT_blackman[m_k_num] ### magnitude (complex raw data)
X_m_k_minus = sig_FFT_blackman[m_k_num-1]
X_m_k_plus = sig_FFT_blackman[m_k_num+1]
C_1 = (X_m_k_minus - X_m_k_plus) / (2 * X_m_k - X_m_k_minus - X_m_k_plus)
freq_estim_1 = np.real(C_1) * freq_bin + m_k ### No windowing
print(freq_estim_1)

m_k_num = sig_freq_blackman.argmax() ### frequency bin 序数
m_k = freqline_blackman[m_k_num] ### 换算为实际频率
X_m_k = sig_freq_blackman[m_k_num] ### magnitude (complex raw data)
X_m_k_minus = sig_freq_blackman[m_k_num-1]
X_m_k_plus = sig_freq_blackman[m_k_num+1]
C_2 = (X_m_k_plus - X_m_k_minus) / (4 * X_m_k - 2 * X_m_k_minus -  2* X_m_k_plus)
freq_estim_2 = C_2 * freq_bin + m_k ### No windowing
print(freq_estim_2)

m_k_num = sig_freq_blackman.argmax() ### frequency bin 序数
m_k = freqline_blackman[m_k_num] ### 换算为实际频率
X_m_k = sig_freq_blackman[m_k_num] ### magnitude (complex raw data)
X_m_k_minus = sig_freq_blackman[m_k_num-1]
X_m_k_plus = sig_freq_blackman[m_k_num+1]
P = 1.22
C_3 = P * (X_m_k_plus - X_m_k_minus) / (X_m_k + X_m_k_minus + X_m_k_plus)
freq_estim_3 = C_3 * freq_bin + m_k ### No windowing
print(freq_estim_3)

m_k_num = sig_freq_blackman.argmax() ### frequency bin 序数
m_k = freqline_blackman[m_k_num] ### 换算为实际频率
X_m_k = sig_FFT_blackman[m_k_num] ### magnitude (complex raw data)
X_m_k_minus = sig_FFT_blackman[m_k_num-1]
X_m_k_plus = sig_FFT_blackman[m_k_num+1]
Q = 0.6
C_4 = Q * (X_m_k_minus - X_m_k_plus) / (2 * X_m_k + X_m_k_minus + X_m_k_plus)
freq_estim_4 = np.real(C_4) * freq_bin + m_k ### No windowing
print(freq_estim_4)

print(FFT_interpolation_boxcar(sig_blackman, tau0)[0])


figure_name = 'Blackman Estimation'
plt.figure(figure_name)
plt.gcf().set_size_inches(8,6)

plt.subplot(3,1,1)
plt.plot(sig, label='Original Signal', color='blue')
plt.plot(sig_blackman, label='Windowed Signal', color='red')
legend = plt.legend(loc='lower right')
plt.setp(legend.get_texts()[0], color = 'blue')
plt.setp(legend.get_texts()[1], color = 'red')

plt.title("Signal Cutting")
plt.ylabel("Amplitude")
plt.xlabel("Samples")
plt.grid(which='major', axis='both')

plt.subplot(3,1,2)
plt.stem(freqline_blackman, sig_freq_blackman, linefmt='r', markerfmt='ro', basefmt=' ', label='Windowed Magnitude Spectrum')
plt.stem(freqline, sig_freq, linefmt='b', markerfmt='bo', basefmt=" ", label='Magnitude Spectrum')
legend = plt.legend(loc='upper right')
plt.setp(legend.get_texts()[0], color = 'red')
plt.setp(legend.get_texts()[1], color = 'blue')
plt.title("Magnitude Spectrum")
plt.ylabel("Magnitude")
plt.xlabel("Frequency")
plt.xlim(0,30)
plt.grid(which='major', axis='both')

plt.tight_layout()
plt.show()